#ifndef __SENSOR_H__
#define __SENSOR_H__

#include "common.h"
#include "mfl.h"  /* Include EASML Math Float Library for filtering */

/* Sensor Configuration */
#define SENSOR_NUM 6u
#define SENSOR_FILTER_TC 0.1f  /* Filter time constant */

/* Sensor Types */
typedef enum {
    SENSOR_TYPE_ULTRASONIC = 0u,
    SENSOR_TYPE_INFRARED = 1u,
    SENSOR_TYPE_TOUCH = 2u,
    SENSOR_TYPE_COLOR = 3u,
    SENSOR_TYPE_SOUND = 4u,
    SENSOR_TYPE_GYRO = 5u
} Sensor_Type_Type;

/* Ultrasonic Sensor Structure */
typedef struct {
    uint16 TrigPin;
    uint16 EchoPin;
    float32 Distance;      /* Distance in mm */
    float32 MaxRange;      /* Maximum range in mm */
    uint32 LastMeasTime;
    boolean IsValid;
} Sensor_Ultrasonic_Type;

/* IR Sensor Structure */
typedef struct {
    uint16 AdcPin;
    uint16 RawValue;
    float32 Distance;      /* Distance in mm */
    float32 Voltage;       /* Voltage in V */
    boolean IsValid;
} Sensor_Infrared_Type;

/* Touch Sensor Structure */
typedef struct {
    uint16 Pin;
    boolean IsPressed;
    boolean LastState;
    uint32 PressTime;
    uint32 ReleaseTime;
} Sensor_Touch_Type;

/* Color Sensor Structure */
typedef struct {
    uint8 I2cAddress;
    uint16 Red;
    uint16 Green;
    uint16 Blue;
    uint16 Clear;
    float32 Lux;
    uint16 ColorTemp;
    boolean IsValid;
} Sensor_Color_Type;

/* Sound Sensor Structure */
typedef struct {
    uint16 AdcPin;
    uint16 RawValue;
    float32 SoundLevel;    /* Sound level in dB */
    float32 Threshold;
    boolean IsTriggered;
} Sensor_Sound_Type;

/* Gyro Sensor Structure */
typedef struct {
    uint8 I2cAddress;
    float32 GyroX;         /* Angular velocity in deg/s */
    float32 GyroY;
    float32 GyroZ;
    float32 AccelX;        /* Acceleration in g */
    float32 AccelY;
    float32 AccelZ;
    float32 Roll;          /* Orientation in degrees */
    float32 Pitch;
    float32 Yaw;
    boolean IsValid;
} Sensor_Gyro_Type;

/* Sensor System Structure */
typedef struct {
    Sensor_Ultrasonic_Type Ultrasonic;
    Sensor_Infrared_Type Infrared;
    Sensor_Touch_Type Touch;
    Sensor_Color_Type Color;
    Sensor_Sound_Type Sound;
    Sensor_Gyro_Type Gyro;
    
    /* Filtering using EASML PT1 filters */
    Mfl_StatePT1_Type FilterStates[SENSOR_NUM];
    float32 FilteredValues[SENSOR_NUM];
    
    uint32 LastUpdateTime;
    boolean SystemReady;
} Sensor_System_Type;

/* Function Declarations */

/* System Initialization */
void Sensor_Init(Sensor_System_Type *System_pst);

/* Ultrasonic Sensor Functions */
void Sensor_UltrasonicInit(Sensor_Ultrasonic_Type *Sensor_pst, 
                          uint16 TrigPin_u16, uint16 EchoPin_u16, float32 MaxRange_f32);
void Sensor_UltrasonicRead(Sensor_Ultrasonic_Type *Sensor_pst);
float32 Sensor_UltrasonicGetDistance(const Sensor_Ultrasonic_Type *Sensor_cpst);

/* IR Sensor Functions */
void Sensor_InfraredInit(Sensor_Infrared_Type *Sensor_pst, uint16 AdcPin_u16);
void Sensor_InfraredRead(Sensor_Infrared_Type *Sensor_pst, uint16 AdcValue_u16);
float32 Sensor_InfraredGetDistance(const Sensor_Infrared_Type *Sensor_cpst);

/* Touch Sensor Functions */
void Sensor_TouchInit(Sensor_Touch_Type *Sensor_pst, uint16 Pin_u16);
void Sensor_TouchUpdate(Sensor_Touch_Type *Sensor_pst, boolean PinState_b, uint32 CurrentTime_u32);
boolean Sensor_TouchIsPressed(const Sensor_Touch_Type *Sensor_cpst);
boolean Sensor_TouchIsReleased(const Sensor_Touch_Type *Sensor_cpst);

/* Color Sensor Functions */
void Sensor_ColorInit(Sensor_Color_Type *Sensor_pst, uint8 I2cAddr_u8);
void Sensor_ColorRead(Sensor_Color_Type *Sensor_pst);
void Sensor_ColorGetRGB(const Sensor_Color_Type *Sensor_cpst, 
                       uint8 *Red_pu8, uint8 *Green_pu8, uint8 *Blue_pu8);
float32 Sensor_ColorGetLux(const Sensor_Color_Type *Sensor_cpst);

/* Sound Sensor Functions */
void Sensor_SoundInit(Sensor_Sound_Type *Sensor_pst, uint16 AdcPin_u16, float32 Threshold_f32);
void Sensor_SoundRead(Sensor_Sound_Type *Sensor_pst, uint16 AdcValue_u16);
boolean Sensor_SoundIsTriggered(const Sensor_Sound_Type *Sensor_cpst);

/* Gyro Sensor Functions */
void Sensor_GyroInit(Sensor_Gyro_Type *Sensor_pst, uint8 I2cAddr_u8);
void Sensor_GyroRead(Sensor_Gyro_Type *Sensor_pst);
void Sensor_GyroGetOrientation(const Sensor_Gyro_Type *Sensor_cpst, 
                              float32 *Roll_pf32, float32 *Pitch_pf32, float32 *Yaw_pf32);

/* Filtering Functions (using EASML) */
void Sensor_FilterInit(Mfl_StatePT1_Type *FilterState_pst, float32 TimeConstant_f32);
float32 Sensor_FilterUpdate(Mfl_StatePT1_Type *FilterState_pst, float32 Input_f32, float32 dT_f32);

/* System Update */
void Sensor_SystemUpdate(Sensor_System_Type *System_pst, uint32 CurrentTime_u32);

/* Utility Functions */
float32 Sensor_AdcToVoltage(uint16 AdcValue_u16, uint16 AdcMax_u16, float32 VRef_f32);
float32 Sensor_VoltageToDistance(float32 Voltage_f32);  /* For IR sensor */

#endif /* __SENSOR_H__ */
